import numpy as np
import matplotlib.pyplot as plt

print("Path Show is Start")

lab_path = ['gps', 'legOdom', 'LioSam', 'LLIO']

# 读取相关数据
gps_path = np.loadtxt('../pathData/sportsGround-04/Groud-truth.txt')
legodom_path = np.loadtxt('../pathData/sportsGround-04/LegOdom_XYZ.txt')
liosam_path = np.loadtxt('../pathData/sportsGround-04/LIOSAM_XYZ.txt')
llio_path = np.loadtxt('../pathData/sportsGround-04/LLIO_XYZ.txt')

fig = plt.figure()
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)

ax1.set_title('Path Show')
ax1.plot(gps_path[:,2], gps_path[:,1], '-', label=lab_path[0])
ax1.plot(legodom_path[:,2], legodom_path[:,1], '-', label=lab_path[1])
ax1.plot(liosam_path[:,2], liosam_path[:,1], '-', label=lab_path[2])
ax1.plot(llio_path[:,2], llio_path[:,1], '-', label=lab_path[3])
ax1.set_aspect(1)
ax1.legend(loc='upper right')
ax1.grid()

ax2.set_title('Path-Z Show')
ax2.plot(gps_path[:,0]-gps_path[0,0], gps_path[:,3], '-', label=lab_path[0])
ax2.plot(legodom_path[:,0]-legodom_path[0,0], legodom_path[:,3], '-', label=lab_path[1])
ax2.plot(liosam_path[:,0]-liosam_path[0,0], liosam_path[:,3], '-', label=lab_path[2])
ax2.plot(llio_path[:,0]-llio_path[0,0], llio_path[:,3], '-', label=lab_path[3])
ax2.grid()
ax2.legend(loc='upper right')
# fig.legend(loc='upper right')

# 设置图片标题
# plt.title('Path Show')
# 绘制曲线 plot(x, y, '线型', label='标签')
# plt.plot(gps_path[:,2], gps_path[:,1], '-', label='gps')
# plt.plot(legodom_path[:,2], legodom_path[:,1], '-', label='legOdom')
# plt.plot(liosam_path[:,2], liosam_path[:,1], '-', label='LioSam')
# plt.plot(llio_path[:,2], llio_path[:,1], '-', label='LLIO')
# plt.gca().set_aspect(1)         # 设置横轴纵轴的尺度统一
# plt.legend(loc='upper right')   # 设置图例（右上角）
# plt.grid()

print("Read Data is Success")
plt.show()